'''
Created on Nov 27, 2012

@author: Himanshu
'''

import math
import numpy as np
global dt
import random
from featureset import gesture

dt = 1.0/125





def calcWorldA(gest):
    """
    comes AFTER removeGrav only!!
    """
    noGestures = gest.returnaNorm().__len__()
    for i in range(noGestures):
        gx = gest.returngX()[i]
        gy = gest.returngY()[i]
        gz = gest.returngZ()[i]
        netaX = gest.netaX[i];
        netaY = gest.netaY[i];
        netaZ = gest.netaZ[i];
        numSamples = gx.__len__()
        waX = [0]*numSamples
        waY = [0]*numSamples
        waZ = [0]*numSamples
        worldTup = gramSchmidt(gest.getStartAccel()[i])
        for j in range(numSamples):
            neta = [netaX[j] , netaY[j] , netaZ[j]]
            waX[j] = project(neta , worldTup[0])
            waY[j] = project(neta , worldTup[1])
            waZ[j] = project(neta , worldTup[2])
            dx = gx[j]*dt
            dy = gy[j]*dt
            dz = gz[j]*dt
            worldTup = rotAll(worldTup , -dx , -dy , -dz)
        gest.worldaX.append(waX)
        gest.worldaY.append(waY)
        gest.worldaZ.append(waZ)
        wx2 = np.square(waX)
        wy2 = np.square(waY)
        wz2 = np.square(waZ)
        gest.worldaNorm.append(np.sqrt(np.add(np.add(wx2 , wy2) , wz2)))
        
def gramSchmidt(vec):
    dim = vec.__len__()
    vecNorm = np.linalg.norm(vec, 2)
    world1 = [0]*dim
    world2 = [0]*dim
    world3 = [0]*dim
    for i in range(dim):
        world1[i] = vec[i]/vecNorm
#        world2[i] = random.random()
#        world3[i] = random.random()
    world2[0] = 1
    world2[1] = 0
    world2[2] = 0
    world3[0] = 0
    world3[1] = 0
    world3[2] = 1
    r12 = np.dot(world1 , world2)
    for i in range(dim):
        world2[i] = world2[i] - r12 * world1[i]
    r22 = np.linalg.norm(world2 , 2)
    for i in range(dim):
        world2[i] = world2[i]/r22
    
    r13 = np.dot(world1 , world3)
    r23 = np.dot(world2 , world3)
    for i in range(dim):
        world3[i] = world3[i] - r13 * world1[i] - r23 * world2[i]
    r33 = np.linalg.norm(world3 , 2)
    for i in range(dim):
        world3[i] = world3[i]/r33
    return [world2 , world3 , world1]     
        
def project(vec1 , vec2):
    return np.dot(vec1 , vec2)/np.linalg.norm(vec2 ,2)

def rotAll(worldTup , dx , dy , dz):
    for i in range(worldTup.__len__()):
        worldTup[i]=rotx(worldTup[i],dx)
        worldTup[i]=roty(worldTup[i],dy)
        worldTup[i]=rotz(worldTup[i],dz)
    return worldTup

def gravComponents(gx , gy , gz , gVec):
    numSamples = gx.__len__()
    currentvec = gVec
    gravX = [0]*numSamples
    gravY = [0]*numSamples
    gravZ = [0]*numSamples   
    gravX[0] = currentvec[0]
    gravY[0] = currentvec[1]
    gravZ[0] = currentvec[2]
    for i in range(1,numSamples):
        gravX[i] = currentvec[0]
        gravY[i] = currentvec[1]
        gravZ[i] = currentvec[2] 
        dx = gx[i-1]*dt
        dy = gy[i-1]*dt
        dz = gz[i-1]*dt
        currentvec=rotx(currentvec,-dx)
        currentvec=roty(currentvec,-dy)
        currentvec=rotz(currentvec,-dz)
        
    return (gravX , gravY , gravZ)

def removeGrav(gest):
    noGestures = gest.returnaX().__len__()
    for i in range(noGestures):
        gravTup = gravComponents(gest.returngX()[i] , gest.returngY()[i] , gest.returngZ()[i] , gest.getStartAccel()[i])
        gest.netaX.append(np.subtract(gest.returnaX()[i] , gravTup[0]))
        gest.netaY.append(np.subtract(gest.returnaY()[i] , gravTup[1]))
        gest.netaZ.append(np.subtract(gest.returnaZ()[i] , gravTup[2]))
        ax2 = np.square(gest.netaX[i])
        ay2 = np.square(gest.netaY[i])
        az2 = np.square(gest.netaZ[i])
        gest.netaNorm.append(np.sqrt(np.add(np.add(ax2 , ay2) , az2)))
        
    
def rotz(invec,dz):
    x=invec[0]
    y=invec[1]
    z=invec[2]
    xp=x*math.cos(dz)-y*math.sin(dz)
    yp=x*math.sin(dz)+y*math.cos(dz)
    zp=z
    outvec=[xp,yp,zp]
    return outvec

def rotx(invec,dx):
    x=invec[0]
    y=invec[1]
    z=invec[2]
    xp=x
    yp=y*math.cos(dx)-z*math.sin(dx)
    zp=y*math.sin(dx)+z*math.cos(dx)
    outvec=[xp,yp,zp]
    return outvec

def roty(invec,dy):
    x=invec[0]
    y=invec[1]
    z=invec[2]
    xp=z*math.sin(dy)+x*math.cos(dy)
    yp=y
    zp=z*math.cos(dy)-x*math.sin(dy)
    outvec=[xp,yp,zp]
    return outvec

def getGestVel(gest):
    noInstances = gest.returnaNorm().__len__()
    vX = []
    vY = []
    vZ = []
    vNorm = []
    for i in range(noInstances):
        aX = gest.netaX[i]
        noFeatures = aX.__len__()
        aY = gest.netaY[i]
        aZ = gest.netaZ[i]
        gX = gest.gX[i]
        gY = gest.gY[i]
        gZ = gest.gZ[i]
        vx = [0]*noFeatures
        vy = [0]*noFeatures
        vz = [0]*noFeatures
        vn = [0]*noFeatures
        for j in range(1,noFeatures):
            currA = [aX[j] , aY[j] , aZ[j]]
            prevA = [aX[j-1] , aY[j-1] , aZ[j-1]]
            dx = gX[j-1]*dt
            dy = gY[j-1]*dt
            dz = gZ[j-1]*dt
            prevA = rotx(prevA , -dx)
            prevA = roty(prevA , -dy)
            prevA = rotz(prevA , -dz)
            avgA = np.divide(np.add(currA , prevA),2)
            dvx = avgA[0]*dt
            dvy = avgA[1]*dt
            dvz = avgA[2]*dt
            prevV = [vx[j-1] , vy[j-1] , vz[j-1]]
            prevV = rotx(prevV , -dx)
            prevV = roty(prevV , -dy)
            prevV = rotz(prevV , -dz)
            vx[j] = prevV[0] + dvx
            vy[j] = prevV[0] + dvy
            vz[j] = prevV[0] + dvz
            vn[j] = np.linalg.norm([vx[j] , vy[j] , vz[j]] , 2)
        vX.append(vx)
        vY.append(vy)
        vZ.append(vz)
        vNorm.append(vn)
    return (vX , vY , vZ , vNorm)

def getGestDisp(gest):
    """ONLY FOR USE AFTER getGestVel!!!"""
    noInstances = gest.returnaNorm().__len__()
    X = []
    Y = []
    Z = []
    distance = []
    velX  = gest.returnVelocities()[0]
    velY = gest.returnVelocities()[1]
    velZ = gest.returnVelocities()[2]
    for i in range(noInstances):
        X.append(getDisp(velX[i]))
        Y.append(getDisp(velY[i]))
        Z.append(getDisp(velZ[i]))
        distance.append(getDist(X[i] , Y[i] , Z[i]))
    return (X , Y , Z , distance)

def getDisp(array):
    p = []
    for v in array:
        dp = v*dt
        p.append(dp)
    return p

def getDist(x , y , z):
    d = []
    dist = 0
    noFeatures = x.__len__()
    for i in range(noFeatures):
        d.append(dist)
        dist += np.linalg.norm([x[i] , y[i] , z[i]])
    return d
    
def getGestEnergy(gest):
    """ONLY FOR USE AFTER getGestvel and getGestPos!!!"""
    noInstances = gest.returnaNorm().__len__()
    E = []
    for i in range(noInstances):
        E.append(getEnergy(gest.netaX[i] , gest.netaY[i] , gest.netaZ[i] , gest.dX[i] , 
                           gest.dY[i] , gest.dZ[i]))
    return E

def getEnergy(ax , ay , az , x , y ,z):
    e = []
    energy = 0
    noSteps = ax.__len__()
    e.append(energy)
    for i in range(1,noSteps):
        dx = x[i]
        dy = y[i]
        dz = z[i]
        energy += (ax[i]*dx + ay[i]*dy + az[i]*dz)
        e.append(energy)
    return e
    
    
    
    
    
    
    
    
    
    
    
    
    